// 8/5 7:15
/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
 * Stack and tested in Gazebo SITL
 */
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/PositionTarget.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TwistStamped.h>
#include <math.h>
#include <std_srvs/Trigger.h>
#include <std_msgs/ColorRGBA.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf/transform_datatypes.h>
#include <mavros_msgs/PositionTarget.h>
#include <std_msgs/Int8.h> 
#include <std_msgs/Int16.h>
#include <std_msgs/Float32.h>
#include "std_msgs/Float32MultiArray.h"
#include "std_msgs/Float64MultiArray.h"
#include <std_msgs/String.h>
#include <stdlib.h>
#include <list>

#define TargeyHeight 1.5
#define TargetYaw 0.2
#define TargetRadius 0.5
#define Limitv_x 0.4  
#define Limitv_y 0.4 
#define Limitv_z 0.3  
#define Limit_yaw 0.2

ros::Subscriber state_sub;//飞机当前状态cur
ros::Subscriber pose_sub;//飞机当前位置信息
ros::Subscriber vel_sub;//飞机速度信息
ros::Publisher local_pose_pub;//设置飞机目标位置（全局坐标
ros::Publisher local_vel_pub;//设置飞机速度（全局坐标
ros::Publisher state_pub;//飞机状态
ros::Publisher set_raw_pub; //以飞机自己为坐标系的速度控制
ros::Publisher state_message_pub;
ros::ServiceClient arming_client;
ros::ServiceClient set_mode_client;
geometry_msgs::TwistStamped vel_cmd; //全局消息，速度设置
geometry_msgs::PoseStamped pose, land_pose;//全局消息，分别用来控制飞机自身的位置与飞机降落add_executable(pole_test src/pole_test.cpp)
mavros_msgs::PositionTarget raw_pose; //全局消息，以飞机自己为坐标系

int land_distance = -1;
double last_error_yaw=0, last_error_dist=0;
double yaw_i = 0 , dist_i = 0;
bool cur_state;
int fire_flag = 0;
bool hold_reset_flag = false;  // 悬停重置标志
bool rotation_reset_flag = false;  // 旋转重置标志

bool state_cb(std_srvs::Trigger::Request &req,std_srvs::Trigger::Response &res){  //用于通过命令启动飞机
    cur_state=!cur_state;
    // 显示请求数据
    ROS_INFO("Publish command [%s]", cur_state==true?"Yes":"No");
	// 设置反馈数据
	res.success = true;
	res.message = "Change drone command state!";
    return true;
}

double min(double a,double b){   //比较ab较小的一个数，其中a的值是被比较值，b是限制量（注意，不可用反
    double ret=0;
    if (a == 0){
        ret=0;
    }else if(fabs(a)>=fabs(b)){
        ret=fabs(b)*a/fabs(a);
    }else{
        ret=fabs(a)*a/fabs(a);
    }
    return ret;
}

// // 使大于180度的yaw角正常化，但此代码中没有用到
// double arc_normalize_yaw(double current_yaw){
//     double raw_yaw=current_yaw;
//     if(fabs(current_yaw)>3.14){
//         raw_yaw = current_yaw > 0 ? current_yaw - 6.28319 : current_yaw + 6.28319;
//     }
//     return raw_yaw;
// }

// 全局函数：重置悬停状态
void reset_hold_state() {
    hold_reset_flag = true;
}

// 全局函数：重置旋转状态
void reset_rotation_state() {
    rotation_reset_flag = true;
}

mavros_msgs::State current_state;  //订阅消息：当前状态
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

geometry_msgs::PoseStamped current_pose; //订阅消息：当前位置（世界坐标系）
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg) {
    current_pose = *msg;
}

geometry_msgs::TwistStamped current_vel; //订阅消息：当前速度
void vel_cb(const geometry_msgs::TwistStamped::ConstPtr& msg)
{
    //ROS_INFO("Current velocity: %.2f m/s", msg->twist.linear.x);
    current_vel = *msg;
}


class move_map
{
private:
    double kpx,kpy,kpz;
    double kix,kiy,kiz;
    double kdx,kdy,kdz;
    double range,base;
    double kp;
    double ki;
    double kd;
    double last_x, last_y, last_z;
    double last_vx, last_vy, last_vz;
    double ix, iy ,iz; 
    double acceleration_limit;

    double radius = 1.0;//半径
    double speed = 0.2;
    double angular_speed = speed / radius;
    double angle = 0.0;
    double rotation_angle;
    double last_angle = 0;

    int interupt_step = 15;
    double height = TargeyHeight;
    geometry_msgs::PoseStamped interupt_pose;


    double func(double error,double A,double B)//变速积分的子函数，主要就是对于积分的判断
    { 
        double result;
        if(error>=B+A){
            result = 0;
        }else if(error<B+A && error>B){
            result = (A+B-error)/A;
        }else{
            result = 1;
        }
        return result;
    }

    //下面的是PID_cal，PID统一计算函数
    //p,i,d为pid的参数，error为输入的误差，A，B为变速积分的参数；
    //B为开始积分的区域，A为区域（如果看不懂的话打我电话，或者全部输入0.5就行了）
    //serial number序号，每个地方应该不一样。
    double PID_cal(double p,double i,double d,double error,
        ros::Rate& rate,double A,double B,int serial_number)
    {
        static double last_error[100] = {0} ;
        static double err_i[100] = {0} ;
        double dt = rate.expectedCycleTime().toSec();
        err_i[serial_number] = error*func(error,A,B)*dt + err_i[serial_number];
        double err_d = (error-last_error[serial_number])*dt;
        double result = error*p + err_i[serial_number]*i + err_d*d;
        last_error[serial_number] = error;
        //ROS_INFO("err_i:%.1lf err_d:%.1lf func_i=%.1lf",err_i,err_d,func(error,A,B));
        return result;
    }


public:
    int fly_to_target(geometry_msgs::PoseStamped target_pose,ros::Publisher& local_pose_pub,ros::Rate& rate) 
    {
        int judge = 0;
        static geometry_msgs::PoseStamped direct_target;
        static bool first_call = true;
        
        // 第一次调用时初始化直接目标为当前位置
        if (first_call) {
            direct_target = current_pose;
            first_call = false;
        }
        
        // 计算到最终目标点的误差
        double ex = target_pose.pose.position.x - current_pose.pose.position.x;
        double ey = target_pose.pose.position.y - current_pose.pose.position.y;
        double ez = target_pose.pose.position.z - current_pose.pose.position.z;
        
        // 计算总误差距离
        double error_distance = sqrt(ex*ex + ey*ey + ez*ez);
        
        // 设置移动速度因子（0-1之间，数值越大移动越快）
        double base_move_factor = 0.02;  // 基础移动因子，可调节
        
        // 实现线性-对数分段函数，以0.5米为分界点优化速度
        double distance_factor;
        const double breakpoint = 0.7;  // 0.5米分界点
        
        if (error_distance <= breakpoint) {
            // 距离 <= 0.5米：使用线性函数增加速度，减少接近时间
            // 线性映射：0m->2.0倍速，0.5m->1.0倍速
            distance_factor = 2.0 - (error_distance / breakpoint);
            distance_factor = std::min(2.0, std::max(1.0, distance_factor));
        } else {
            // 距离 > 0.5米：使用对数函数抑制速度，防止过冲
            // 对数映射：0.5m->1.0倍速，随距离增加逐渐减少到0.3倍速
            double log_factor = std::log(error_distance / breakpoint + 1.0);
            distance_factor = 1.0 / (1.0 + 0.7 * log_factor);  // 最低降到约0.3倍速
            distance_factor = std::min(1.0, std::max(0.3, distance_factor));
        }
        
        // 应用距离因子到各轴移动因子
        double move_factor_x = base_move_factor * distance_factor;
        double move_factor_y = base_move_factor * distance_factor;
        double move_factor_z = base_move_factor * distance_factor;
        
        // 限制最小移动因子，防止停滞
        move_factor_x = std::max(0.005, move_factor_x);
        move_factor_y = std::max(0.005, move_factor_y);
        move_factor_z = std::max(0.005, move_factor_z);
        
        // 更新直接目标位置，使用您提供的限幅公式
        direct_target.header.stamp = ros::Time::now();
        direct_target.header.frame_id = "map";
        
        // 位置限幅更新
        direct_target.pose.position.x += move_factor_x * (target_pose.pose.position.x - direct_target.pose.position.x);
        direct_target.pose.position.y += move_factor_y * (target_pose.pose.position.y - direct_target.pose.position.y);
        direct_target.pose.position.z += move_factor_z * (target_pose.pose.position.z - direct_target.pose.position.z);
        
        // 防止超调：如果direct_target超过了目标位置，则直接设置为目标位置
        if ((target_pose.pose.position.x - current_pose.pose.position.x) * 
            (target_pose.pose.position.x - direct_target.pose.position.x) <= 0) {
            direct_target.pose.position.x = target_pose.pose.position.x;
        }
        if ((target_pose.pose.position.y - current_pose.pose.position.y) * 
            (target_pose.pose.position.y - direct_target.pose.position.y) <= 0) {
            direct_target.pose.position.y = target_pose.pose.position.y;
        }
        if ((target_pose.pose.position.z - current_pose.pose.position.z) * 
            (target_pose.pose.position.z - direct_target.pose.position.z) <= 0) {
            direct_target.pose.position.z = target_pose.pose.position.z;
        }
        
        // 设置目标姿态 - 如果目标姿态未指定，保持当前姿态
        if (target_pose.pose.orientation.w == 0.0 && 
            target_pose.pose.orientation.x == 0.0 && 
            target_pose.pose.orientation.y == 0.0 && 
            target_pose.pose.orientation.z == 0.0) {
            // 目标姿态未设置，保持当前姿态
            direct_target.pose.orientation = current_pose.pose.orientation;
        } else {
            // 使用指定的目标姿态
            direct_target.pose.orientation = target_pose.pose.orientation;
        }
        
        // 发布限幅后的位置指令
        local_pose_pub.publish(direct_target);
        
        // 输出调试信息 - 修复版本
        static ros::Time last_debug_time;
        static bool debug_time_initialized = false;
        if (!debug_time_initialized) {
            last_debug_time = ros::Time::now();
            debug_time_initialized = true;
        }
        
        if ((ros::Time::now() - last_debug_time).toSec() >= 2.0) {  // 每2秒输出一次
            ROS_INFO("Final Target: [%.3f, %.3f, %.3f], Direct Target: [%.3f, %.3f, %.3f], Current: [%.3f, %.3f, %.3f]", 
                     target_pose.pose.position.x, target_pose.pose.position.y, target_pose.pose.position.z,
                     direct_target.pose.position.x, direct_target.pose.position.y, direct_target.pose.position.z,
                     current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z);
            ROS_INFO("Error Distance: %.3f m, Distance Factor: %.3f (%.1fm breakpoint), Move Factors: x=%.3f, y=%.3f, z=%.3f", 
                     error_distance, distance_factor, breakpoint, move_factor_x, move_factor_y, move_factor_z);
            last_debug_time = ros::Time::now();
        }
        
        // 判断是否到达目标点 - 增加时间限制
        static ros::Time target_reached_start_time;
        static bool target_reached_timing = false;
        static bool target_reached_time_initialized = false;
        
        if (fabs(ex) < 0.1 && fabs(ey) < 0.1 && fabs(ez) < 0.1) {
            if (!target_reached_timing) {
                // 第一次进入目标范围，开始计时
                target_reached_start_time = ros::Time::now();
                target_reached_timing = true;
                target_reached_time_initialized = true;
                ROS_INFO("Entered target zone, starting 2-second confirmation timer...");
            } else {
                // 已经在计时中，检查是否满足1.5秒条件
                double time_in_target = (ros::Time::now() - target_reached_start_time).toSec();
                if (time_in_target >= 1.5) {
                    judge = 1;
                    first_call = true;  // 重置状态为下次使用
                    target_reached_timing = false;  // 重置计时状态
                    debug_time_initialized = false;  // 重置调试时间初始化标志
                    ROS_INFO("Target reached! Confirmed for %.2f seconds. Resetting for next waypoint.", time_in_target);
                } else {
                    ROS_INFO("In target zone for %.2f/2.0 seconds...", time_in_target);
                }
            }
        } else {
            // 离开了目标范围，重置计时
            if (target_reached_timing) {
                target_reached_timing = false;
                ROS_INFO("Left target zone, resetting timer. Distance: %.3f m", error_distance);
            }
        }
        
        return judge;
    }

    //对准圆心函数
    int circle_locator(std_msgs::ColorRGBA RGBA,ros::Publisher& local_vel_pub,ros::Rate& rate)
    {    
        int judge = 0;
        double err_x = -RGBA.g/300; 
        double err_y = -RGBA.r/300;
        ROS_INFO("err = %.2lf,err = %.2lf", err_x, err_y);
        double dt = rate.expectedCycleTime().toSec();
        double ez = height - current_pose.pose.position.z;
        ix += err_x * dt * func(err_x,range,base);
        iy += err_y * dt * func(err_y,range,base);
        iz += ez * dt * func(ez,range,base);
        ix = min(ix,0.1);
        iy = min(iy,0.1);
        iz = min(iz,0.1);
        double dx = (err_x - last_x) / dt;
        double dy = (err_y - last_y) / dt;
        double dz = (ez - last_z) / dt;
        double vx = kp * err_x + ki * ix + kd * dx;
        double vy = kp * err_y + ki * iy + kd * dy;
        double vz = kpz * ez + kiz * iz + kdz * dz;
        geometry_msgs::TwistStamped vel;
        vel.header.stamp = ros::Time::now();
        vel.twist.linear.x = min(vx,Limitv_x);
        vel.twist.linear.y = min(vy,Limitv_y);
        vel.twist.linear.z = min(vz,Limitv_z);
        ROS_INFO("vx = %.2lf,vy = %.2lf,vz = %.2lf", vel.twist.linear.x, vel.twist.linear.y, vel.twist.linear.z);
        vel.twist.angular.x = 0.0;
        vel.twist.angular.y = 0.0;
        vel.twist.angular.z = 0.0;
        // 发布速度指令 
        local_vel_pub.publish(vel);
        // 判断是否到达目标点
        return judge;
    }

    //基于速度的速度控制：设置飞机的速度，其中x,y,z都是飞机以自己为坐标系的速度，yaw_rate是偏航角旋转速率
    void set_speed_body(double x, double y, double z,  double yaw_rate ) 
    {
        mavros_msgs::PositionTarget raw_target;
        geometry_msgs::Twist vz0;
        raw_target.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_NED;
        raw_target.type_mask = mavros_msgs::PositionTarget::IGNORE_PX | 
        mavros_msgs::PositionTarget::IGNORE_PY | 
        //mavros_msgs::PositionTarget::IGNORE_PZ | 
        mavros_msgs::PositionTarget::IGNORE_AFX | 
        mavros_msgs::PositionTarget::IGNORE_AFY | 
        mavros_msgs::PositionTarget::IGNORE_AFZ | 
        mavros_msgs::PositionTarget::IGNORE_YAW;
        if (fabs(yaw_rate) < 1e-2)
        raw_target.type_mask |= mavros_msgs::PositionTarget::IGNORE_YAW_RATE;
        ROS_INFO("body_speed:%.1lf %.1lf %.1lf",x,y,yaw_rate);
        raw_target.velocity.x = x;
        raw_target.velocity.y = y;
        raw_target.velocity.z = z;
        raw_target.yaw_rate =yaw_rate;//yaw_rate * 0.01745329;
        set_raw_pub.publish(raw_target);
    }

    //基于位置的速度控制：设置飞机的速度，其中x,y都是飞机以世界为坐标系的速度，yaw_rate是偏航角旋转速率，z默认为目标高度
    void pose_velocity_ctrl(double vx, double vy, double vel_limit_x,double vel_limit_y, double yaw_rate, double yaw_limit, ros::Rate& rate )
    {   
        geometry_msgs::PoseStamped position=current_pose;
        // double current_time = ros::Time::now().toSec();
        // double elapsed_time = current_time - yaw_last_time;
        // yaw_last_time = current_time;
        double dt = rate.expectedCycleTime().toSec();
        vx=min(vx,vel_limit_x);
        vy=min(vy,vel_limit_y);
        yaw_rate = min(yaw_rate,yaw_limit);
        position.header.stamp = ros::Time::now();
        position.header.frame_id = "map";
        // ROS_INFO(" vx = %.2lf vy = %.2lf " ,vx , vy);
        double current_yaw = tf::getYaw(current_pose.pose.orientation);// Set the new yaw angle
        double new_yaw = current_yaw + yaw_rate * dt;
        tf::Quaternion q_tf = tf::createQuaternionFromYaw(new_yaw);
        geometry_msgs::Quaternion current_q;
        tf::quaternionTFToMsg(q_tf, current_q);
        position.pose.position.x = current_pose.pose.position.x + vx*dt;
        position.pose.position.y = current_pose.pose.position.y + vy*dt;
        position.pose.position.z = TargeyHeight; 
        position.pose.orientation = current_q;
        local_pose_pub.publish(position);
    }

    // 基于位置的位置控制：设置飞机的位置和姿态
    void pose_position_ctrl(double target_x, double target_y, double target_z, double target_yaw)
    {   
        geometry_msgs::PoseStamped position;
        position.header.stamp = ros::Time::now();
        position.header.frame_id = "map";
        
        // 设置目标位置
        position.pose.position.x = target_x;
        position.pose.position.y = target_y;
        position.pose.position.z = target_z;
        
        // 设置目标姿态
        tf::Quaternion q_tf = tf::createQuaternionFromYaw(target_yaw);
        geometry_msgs::Quaternion target_q;
        tf::quaternionTFToMsg(q_tf, target_q);
        position.pose.orientation = target_q;
        
        local_pose_pub.publish(position);
    }

    // 基于位置的悬停，稳，放心使用
    void hold(ros::Publisher& local_pose_pub)
    {
        static geometry_msgs::PoseStamped fixed_hold_pose;
        static bool hold_initialized = false;
        
        // 检查是否需要重置悬停状态
        extern bool hold_reset_flag;
        if (hold_reset_flag) {
            hold_initialized = false;
            hold_reset_flag = false;
        }
        
        // 第一次调用时或重置后记录悬停位置
        if (!hold_initialized) {
            fixed_hold_pose = current_pose;
            fixed_hold_pose.header.stamp = ros::Time::now();
            fixed_hold_pose.header.frame_id = "map";
            hold_initialized = true;
            ROS_INFO("Hold position locked at [%.3f, %.3f, %.3f]", 
                     fixed_hold_pose.pose.position.x, 
                     fixed_hold_pose.pose.position.y, 
                     fixed_hold_pose.pose.position.z);
        }
        
        // 更新时间戳但保持固定位置
        fixed_hold_pose.header.stamp = ros::Time::now();
        
        // 发布固定的位置命令
        local_pose_pub.publish(fixed_hold_pose);
    }
    
    // 重置悬停状态（用于切换到新的航路点时）
    void reset_hold()
    {
        static bool* hold_initialized_ptr = nullptr;
        if (hold_initialized_ptr == nullptr) {
            // 获取hold函数中静态变量的指针（通过调用一次hold来初始化）
            static bool reset_flag = true;
            if (reset_flag) {
                reset_flag = false;
                // 这里我们通过重新设计来避免直接访问静态变量
            }
        }
        // 由于C++的限制，我们在WaypointManager中处理重置
    }

    int pole_rotation(geometry_msgs::PoseStamped center_pose,ros::Publisher& local_vel_pub,ros::Rate& rate)
    {
        int flag = 0;
        double dt = rate.expectedCycleTime().toSec();
        angle += dt * angular_speed;
        //ROS_INFO("target_angle = %.2lf",angle);
        double px = radius * cos(angle)+center_pose.pose.position.x;
        double py = radius * sin(angle)+center_pose.pose.position.y;
        if(sqrt((px - current_pose.pose.position.x)*(px - current_pose.pose.position.x)+
        (py - current_pose.pose.position.y)*(py - current_pose.pose.position.y)
        )>0.3){
            angle -= dt * angular_speed * 3;
            //ROS_INFO("angle too large tar_angle = %.2lf",angle);
            pose.pose.position.x = radius * cos(last_angle)+center_pose.pose.position.x;
            pose.pose.position.y = radius * sin(last_angle)+center_pose.pose.position.y;
            pose.pose.position.z = TargeyHeight;
            local_pose_pub.publish(pose);
        }else{
        pose.pose.position.x = radius * cos(angle)+center_pose.pose.position.x;
        pose.pose.position.y = radius * sin(angle)+center_pose.pose.position.y;
        pose.pose.position.z = TargeyHeight;
        last_angle = angle;
        local_pose_pub.publish(pose);
        }        
        if(fabs(angle - rotation_angle) < 0.05){
            ROS_INFO("angle = %.2lf , current_angle = %.2lf",angle,rotation_angle);
            flag = 1;
        }
        return flag;
    }

    void set_pid_xy(double p,double i,double d){
        kp = p;
        ki = i;
        kd = d;
    }

    void set_pid_z(double p,double i,double d){
        kpz = p;
        kiz = i;
        kdz = d;
    }

    void pid_init(){
        last_x = 0.0, last_y = 0.0, last_z = 0.0;
        ix = 0.0, iy = 0.0, iz = 0.0; 
        last_vx = 0.0, last_vy = 0.0, last_vz = 0.0;
    }

    void set_acceleration_limit(double a){
        acceleration_limit = a;
    }

    void print_pose(){
        ROS_INFO("px = %.2lf,py = %.2lf,pz = %.2lf",
        current_pose.pose.position.x,current_pose.pose.position.y,current_pose.pose.position.z);
    }

    void print_pid(){
        ROS_INFO("ix = %.2lf,iy = %.2lf,iz = %.2lf",
        ix,iy,iz);
    }    

    void set_height(double height_input){
        height = height_input;
    }

    int get_interupt_step(){
        return interupt_step;
    }

    geometry_msgs::PoseStamped get_interupt_pose(){
        return interupt_pose;
    }


    void circle_init(double r,double linear_vel,double current_angle,double spin_angle){
        radius = r;//半径
        speed = linear_vel;
        angular_speed = speed / radius;
        angle = current_angle;
        rotation_angle = spin_angle*min(linear_vel,0.1)*10 + current_angle;
        last_angle = current_angle;
        ROS_INFO("circle init: r= %.2lf,current_angle = %.2lf",r,current_angle);
    }
};

// 动作类型枚举
enum ActionType {
    MOVE_TO_POINT,      // 移动到指定点
    ROTATE,             // 旋转到指定角度
    TOPIC_CONTROL,      // 接收话题控制
    CIRCLE,             // 圆形飞行
    HOVER,              // 悬停
    WAIT,               // 等待指定时间
    SAFE_LANDING        // 安全降落（位置控制到30cm后自动降落）
};

// 增强版航路点结构体
struct Waypoint {
    ActionType actionType;
    double x, y, z;              // 位置坐标
    double yaw;                  // 目标偏航角
    double duration;             // 持续时间(秒)
    double speed;                // 移动速度
    std::string stepName;        // 步骤名称
    std::string topicName;       // 话题名称(用于TOPIC_CONTROL)
    double radius;               // 圆形飞行半径
    
    // 构造函数 - 移动到点
    Waypoint(double x_val, double y_val, double z_val, const std::string& name) 
        : actionType(MOVE_TO_POINT), x(x_val), y(y_val), z(z_val), yaw(0), 
          duration(0), speed(0.5), stepName(name), radius(0) {}
    
    // 构造函数 - 旋转
    Waypoint(double target_yaw, const std::string& name, bool is_rotation) 
        : actionType(ROTATE), x(0), y(0), z(0), yaw(target_yaw), 
          duration(0), speed(0), stepName(name), radius(0) {}
    
    // 构造函数 - 话题控制
    Waypoint(const std::string& topic_name, double control_duration, const std::string& name) 
        : actionType(TOPIC_CONTROL), x(0), y(0), z(0), yaw(0), 
          duration(control_duration), speed(0), stepName(name), topicName(topic_name), radius(0) {}
    
    // 构造函数 - 圆形飞行
    Waypoint(double center_x, double center_y, double center_z, double fly_radius, 
             double circle_duration, const std::string& name) 
        : actionType(CIRCLE), x(center_x), y(center_y), z(center_z), yaw(0), 
          duration(circle_duration), speed(0.5), stepName(name), radius(fly_radius) {}
    
    // 构造函数 - 悬停/等待
    Waypoint(double hover_duration, const std::string& name, ActionType type) 
        : actionType(type), x(0), y(0), z(0), yaw(0), 
          duration(hover_duration), speed(0), stepName(name), radius(0) {}
    
    // 构造函数 - 安全降落
    Waypoint(const std::string& name, bool is_safe_landing) 
        : actionType(SAFE_LANDING), x(0), y(0), z(0), yaw(0), 
          duration(0), speed(0.2), stepName(name), radius(0) {}  // speed用作下降速率
};

// 增强版航路点管理类
class WaypointManager {
private:
    std::list<Waypoint> waypoints;
    std::list<Waypoint>::iterator currentWaypoint;
    bool initialized;
    ros::Time actionStartTime;
    bool actionInProgress;
    double initialYaw;
    geometry_msgs::PoseStamped topicControlMsg;
    ros::Subscriber topicControlSub;
    bool hasTopicControl;

public:
    WaypointManager() : initialized(false), actionInProgress(false), hasTopicControl(false) {
        // initializeWaypoints();
    }
    
    // void initializeWaypoints() {
    //     waypoints.clear();
        
    //     // 示例航路点序列 - 展示各种动作类型
    //     waypoints.emplace_back(2.82, 0, 1.50, "step2_move");                    // 移动到点
    //     waypoints.emplace_back(M_PI/4, "step3_rotate", true);                   // 旋转45度
    //     waypoints.emplace_back(3.55, -0.43, 1.50, "step4_move");               // 移动到点
    //     waypoints.emplace_back("/manual_control", 10.0, "step5_topic_control"); // 话题控制10秒
    //     waypoints.emplace_back(3.225, -1.165, 1.50, 1.0, 5.0, "step6_circle"); // 圆形飞行
    //     waypoints.emplace_back(3.0, "step7_hover", HOVER);                      // 悬停3秒
    //     waypoints.emplace_back(2.82, -2, 1.50, "step8_move");                  // 最终移动
        
    //     currentWaypoint = waypoints.begin();
    //     initialized = true;
    // }
    
    // 话题控制回调函数
    void topicControlCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
        topicControlMsg = *msg;
        hasTopicControl = true;
    }
    
    // 设置话题订阅
    void setupTopicSubscription(ros::NodeHandle& nh, const std::string& topicName) {
        topicControlSub = nh.subscribe<geometry_msgs::PoseStamped>(
            topicName, 10, &WaypointManager::topicControlCallback, this);
    }
    
    bool hasNextWaypoint() {
        return initialized && (currentWaypoint != waypoints.end());
    }
    
    Waypoint getCurrentWaypoint() {
        if (currentWaypoint != waypoints.end()) {
            return *currentWaypoint;
        }
        return Waypoint(0, 0, 0, "invalid");
    }
    
    void moveToNextWaypoint() {
        if (currentWaypoint != waypoints.end()) {
            ++currentWaypoint;
            actionInProgress = false;  // 重置动作状态
        }
    }
    
    // 开始当前动作
    void startCurrentAction() {
        actionStartTime = ros::Time::now();
        actionInProgress = true;
        
        // 为不同动作类型进行特殊初始化
        Waypoint current = getCurrentWaypoint();
        if (current.actionType == ROTATE) {
            initialYaw = tf::getYaw(current_pose.pose.orientation);
            // 重置旋转状态，确保每次旋转都锁定新的位置
            resetRotationPosition();
        } else if (current.actionType == HOVER || current.actionType == WAIT) {
            // 重置悬停状态，确保每次悬停都锁定新的位置
            resetHoldPosition();
        }
    }
    
    // 重置悬停位置锁定状态
    void resetHoldPosition() {
        // 由于C++静态变量的限制，我们通过一个特殊的方法来重置
        // 这个方法会在下一次调用hold()时强制重新初始化位置
        extern void reset_hold_state();  // 声明外部函数
        reset_hold_state();
    }
    
    // 重置旋转位置锁定状态
    void resetRotationPosition() {
        // 由于C++静态变量的限制，我们通过一个特殊的方法来重置
        // 这个方法会在下一次调用performRotation()时强制重新初始化位置
        extern void reset_rotation_state();  // 声明外部函数
        reset_rotation_state();
    }
    
    // 检查当前动作是否完成
    bool isCurrentActionComplete(move_map& move1, ros::Publisher& local_vel_pub, ros::Rate& rate) {
        if (!actionInProgress) {
            startCurrentAction();
            return false;
        }
        
        Waypoint current = getCurrentWaypoint();
        ros::Duration elapsed = ros::Time::now() - actionStartTime;
        
        switch (current.actionType) {
            case MOVE_TO_POINT: {
                geometry_msgs::PoseStamped target;
                target.pose.position.x = current.x;
                target.pose.position.y = current.y;
                target.pose.position.z = current.z;
                return move1.fly_to_target(target, local_pose_pub, rate) == 1;
            }
            
            case ROTATE: {
                return performRotation(current.yaw, move1, rate);
            }
            
            case TOPIC_CONTROL: {
                if (elapsed.toSec() >= current.duration) {
                    return true;  // 时间到了，结束话题控制
                }
                performTopicControl(move1, rate);
                return false;
            }
            
            case CIRCLE: {
                if (elapsed.toSec() >= current.duration) {
                    return true;
                }
                performCircleFlight(current, move1, rate, elapsed.toSec());
                return false;
            }
            
            case HOVER:
            case WAIT: {
                move1.hold(local_pose_pub);  // 使用位置发布器
                return elapsed.toSec() >= current.duration;
            }
            
            case SAFE_LANDING: {
                return performSafeLanding(current, move1, rate);
            }
            
            default:
                return true;
        }
    }
    
private:
    // 执行旋转动作 - 缓慢旋转且固定位置
    bool performRotation(double targetYaw, move_map& move1, ros::Rate& rate) {
        static double lastTargetYaw = -999.0;  // 使用特殊值标识未初始化
        static double currentTargetYaw = 0.0;
        static geometry_msgs::PoseStamped fixedPosition;  // 固定旋转位置
        static bool rotationInitialized = false;
        
        double currentYaw = tf::getYaw(current_pose.pose.orientation);
        double yawError = targetYaw - currentYaw;
        
        // 角度归一化到[-π, π]
        while (yawError > M_PI) yawError -= 2 * M_PI;
        while (yawError < -M_PI) yawError += 2 * M_PI;
        
        // 检查是否需要重置旋转状态
        extern bool rotation_reset_flag;
        if (rotation_reset_flag) {
            rotationInitialized = false;
            rotation_reset_flag = false;
        }
        
        // 初始化旋转 - 改进版本
        if (!rotationInitialized || fabs(targetYaw - lastTargetYaw) > 0.01) {
            // 锁定当前位置作为旋转中心
            fixedPosition = current_pose;
            fixedPosition.header.stamp = ros::Time::now();
            fixedPosition.header.frame_id = "map";
            currentTargetYaw = currentYaw;
            rotationInitialized = true;
            lastTargetYaw = targetYaw;
            
            ROS_INFO("Rotation position locked at [%.3f, %.3f, %.3f]", 
                     fixedPosition.pose.position.x, fixedPosition.pose.position.y, fixedPosition.pose.position.z);
            ROS_INFO("Starting rotation from %.2f to %.2f degrees", 
                     currentYaw * 180.0 / M_PI, targetYaw * 180.0 / M_PI);
        }
        
        // 计算旋转步长：增大到30度/秒，减少调整频率
        double dt = rate.expectedCycleTime().toSec();
        double maxRotationStep = 0.523 * dt;  // 30度/秒 = 0.523弧度/秒
        
        // 计算到目标角度的误差
        double remainingError = targetYaw - currentTargetYaw;
        while (remainingError > M_PI) remainingError -= 2 * M_PI;
        while (remainingError < -M_PI) remainingError += 2 * M_PI;
        
        // 限制每个控制周期的旋转量
        if (fabs(remainingError) > maxRotationStep) {
            currentTargetYaw += (remainingError > 0) ? maxRotationStep : -maxRotationStep;
        } else {
            currentTargetYaw = targetYaw;  // 已经接近目标，直接设置
        }
        
        // 使用位置控制进行旋转 - 严格固定位置版本
        geometry_msgs::PoseStamped rotation_pose;
        rotation_pose.header.stamp = ros::Time::now();
        rotation_pose.header.frame_id = "map";
        
        // 严格使用锁定的位置，完全不使用当前位置
        rotation_pose.pose.position.x = fixedPosition.pose.position.x;
        rotation_pose.pose.position.y = fixedPosition.pose.position.y;
        rotation_pose.pose.position.z = fixedPosition.pose.position.z;
        
        // 设置当前步骤的目标偏航角
        tf::Quaternion q_tf = tf::createQuaternionFromYaw(currentTargetYaw);
        geometry_msgs::Quaternion target_q;
        tf::quaternionTFToMsg(q_tf, target_q);
        rotation_pose.pose.orientation = target_q;
        
        // 发布位置命令
        local_pose_pub.publish(rotation_pose);
        
        // 检查是否到达目标 - 增大容忍度
        if (fabs(yawError) < 0.047) {  // 2.5度误差内认为到达 (0.047弧度≈2.5度)
            rotationInitialized = false;  // 重置状态以备下次使用
            lastTargetYaw = -999.0;  // 重置标识
            ROS_INFO("Rotation completed! Final yaw: %.2f degrees at fixed position [%.3f, %.3f, %.3f]", 
                     currentYaw * 180.0 / M_PI,
                     fixedPosition.pose.position.x, fixedPosition.pose.position.y, fixedPosition.pose.position.z);
            return true;
        }
        
        // 每2秒输出一次调试信息
        static ros::Time last_debug_time;
        static bool debug_initialized = false;
        if (!debug_initialized) {
            last_debug_time = ros::Time::now();
            debug_initialized = true;
        }
        
        if ((ros::Time::now() - last_debug_time).toSec() >= 2.0) {
            ROS_INFO("Rotating: current=%.1f°, target=%.1f°, error=%.1f°", 
                     currentYaw * 180.0 / M_PI, targetYaw * 180.0 / M_PI, yawError * 180.0 / M_PI);
            ROS_INFO("Fixed position: [%.3f, %.3f, %.3f], Current position: [%.3f, %.3f, %.3f]",
                     fixedPosition.pose.position.x, fixedPosition.pose.position.y, fixedPosition.pose.position.z,
                     current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z);
            last_debug_time = ros::Time::now();
        }
        
        return false;
    }
    
    // 执行话题控制
    void performTopicControl(move_map& move1, ros::Rate& rate) {
        if (!hasTopicControl) {
            move1.pose_velocity_ctrl(0, 0, 0, 0, 0, 0, rate);  // 无控制信号时悬停
            return;
        }
        
        // 根据话题消息控制移动
        double vx = topicControlMsg.pose.position.x * 0.4;  // 1.0对应0.4m/s
        double vy = topicControlMsg.pose.position.y * 0.4;  // 1.0对应0.4m/s
        
        // 速度限幅
        vx = std::max(-0.4, std::min(0.4, vx));
        vy = std::max(-0.4, std::min(0.4, vy));
        
        move1.pose_velocity_ctrl(vx, vy, 0.4, 0.4, 0, 0, rate);
        hasTopicControl = false;  // 重置控制标志
    }
    
    // 执行圆形飞行
    void performCircleFlight(const Waypoint& current, move_map& move1, ros::Rate& rate, double elapsedTime) {
        double angularSpeed = 0.5;  // 角速度 rad/s
        double angle = angularSpeed * elapsedTime;
        
        double targetX = current.x + current.radius * cos(angle);
        double targetY = current.y + current.radius * sin(angle);
        
        geometry_msgs::PoseStamped target;
        target.pose.position.x = targetX;
        target.pose.position.y = targetY;
        target.pose.position.z = current.z;
        
        move1.fly_to_target(target, local_vel_pub, rate);
    }
    
    // 执行安全降落 - 位置控制到30cm后AUTO.LAND
    bool performSafeLanding(const Waypoint& current, move_map& move1, ros::Rate& rate) {
        static double landing_target_height = 0.0;
        static bool landing_initialized = false;
        static bool auto_land_triggered = false;
        
        if (!landing_initialized) {
            landing_target_height = current_pose.pose.position.z;
            landing_initialized = true;
            auto_land_triggered = false;
            ROS_WARN("Starting safe landing from %.2f meters", landing_target_height);
        }
        
        // 第一阶段：使用fly_to_target函数缓慢降落到20cm
        if (!auto_land_triggered) {
            // 缓慢下降速度：使用speed字段作为下降速率
            double descent_rate = current.speed; // 默认0.2m/s
            double dt = rate.expectedCycleTime().toSec();
            landing_target_height -= descent_rate * dt;
            
            // 限制最低高度为20cm
            if (landing_target_height < 0.13) {
                landing_target_height = 0.10;
            }
            
            // 使用fly_to_target函数进行精确降落控制
            geometry_msgs::PoseStamped landing_target;
            landing_target.header.stamp = ros::Time::now();
            landing_target.header.frame_id = "map";
            landing_target.pose.position.x = current_pose.pose.position.x;
            landing_target.pose.position.y = current_pose.pose.position.y;
            landing_target.pose.position.z = landing_target_height;
            landing_target.pose.orientation = current_pose.pose.orientation;
            
            // 使用fly_to_target函数实现平滑降落
            int landing_result = move1.fly_to_target(landing_target, local_pose_pub, rate);
            
            // 输出降落调试信息
            static ros::Time last_landing_debug_time;
            static bool landing_debug_initialized = false;
            if (!landing_debug_initialized) {
                last_landing_debug_time = ros::Time::now();
                landing_debug_initialized = true;
            }
            
            if ((ros::Time::now() - last_landing_debug_time).toSec() >= 2.0) {
                ROS_INFO("Landing: Current height: %.2fm, Target height: %.2fm, Speed: %.2fm/s", 
                         current_pose.pose.position.z, landing_target_height, descent_rate);
                last_landing_debug_time = ros::Time::now();
            }
            
            // 当接近13cm时，切换到AUTO.LAND
            if (current_pose.pose.position.z <= 0.13) {
                auto_land_triggered = true;
                landing_debug_initialized = false;  // 重置调试状态
                ROS_WARN("Switching to AUTO.LAND at %.2f meters", current_pose.pose.position.z);
            }
            
            return false;
        }
        
        // 第二阶段：AUTO.LAND最终降落
        static ros::Time land_request_time = ros::Time::now();
        mavros_msgs::SetMode land_mode;
        land_mode.request.custom_mode = "AUTO.LAND";
        
        if (current_state.mode != "AUTO.LAND" && 
            (ros::Time::now() - land_request_time > ros::Duration(2.0))) {
            if (set_mode_client.call(land_mode) && land_mode.response.mode_sent) {
                ROS_INFO("AUTO.LAND enabled for final touchdown");
                // 重置状态以备下次使用
                landing_initialized = false;
                auto_land_triggered = false;
                return true;  // 降落完成
            }
            land_request_time = ros::Time::now();
        }
        
        return false;
    }

public:
    // 便捷的航路点添加方法
    void addMovePoint(double x, double y, double z, const std::string& name) {
        waypoints.emplace_back(x, y, z, name);
    }
    
    void addRotation(double yaw, const std::string& name) {
        waypoints.emplace_back(yaw, name, true);
    }
    
    void addTopicControl(const std::string& topicName, double duration, const std::string& name) {
        waypoints.emplace_back(topicName, duration, name);
    }
    
    void addCircleFlight(double centerX, double centerY, double centerZ, 
                        double radius, double duration, const std::string& name) {
        waypoints.emplace_back(centerX, centerY, centerZ, radius, duration, name);
    }
    
    void addHover(double duration, const std::string& name) {
        waypoints.emplace_back(duration, name, HOVER);
    }
    
    void addWait(double duration, const std::string& name) {
        waypoints.emplace_back(duration, name, WAIT);
    }
    
    void addSafeLanding(const std::string& name) {
        waypoints.emplace_back(name, true);  // 使用安全降落构造函数
    }
    
    void reset() {
        waypoints.clear();
        actionInProgress = false;
        initialized = false;
    }
    
    void finalize() {
        if (!waypoints.empty()) {
            currentWaypoint = waypoints.begin();
            initialized = true;
            ROS_INFO("WaypointManager initialized with %lu waypoints", waypoints.size());
            
            // 打印所有航路点信息
            int index = 0;
            for (auto it = waypoints.begin(); it != waypoints.end(); ++it) {
                ROS_INFO("Waypoint %d: %s (Type: %d)", index++, it->stepName.c_str(), it->actionType);
            }
        } else {
            ROS_WARN("No waypoints to initialize!");
        }
    }
    
    // 获取当前动作类型
    ActionType getCurrentActionType() {
        if (currentWaypoint != waypoints.end()) {
            return currentWaypoint->actionType;
        }
        return MOVE_TO_POINT;
    }
};

// 示例：创建复杂任务序列的函数
void createComplexMission(WaypointManager& manager) {
    // 清空现有航路点
    manager.reset();
    
    // 任务序列：搜索和救援模式
    // 0. 起飞到任务高度
    manager.addMovePoint(0.0, 0.0, 2.0, "takeoff_to_mission_height");
    
    // 1. 移动到搜索区域起点
    manager.addMovePoint(5.0, 0.0, 2.0, "move_to_search_start");
    
    // 2. 旋转到搜索方向
    manager.addRotation(0.0, "face_north");
    
    // 3. 搜索区域圆形飞行
    manager.addCircleFlight(5.0, 0.0, 2.0, 3.0, 15.0, "search_pattern_1");
    
    // 4. 移动到第二搜索点
    manager.addMovePoint(10.0, 5.0, 2.0, "move_to_search_2");
    
    // 5. 第二个搜索圆形飞行
    manager.addCircleFlight(10.0, 5.0, 2.0, 2.0, 12.0, "search_pattern_2");
    
    // 6. 悬停等待指令
    manager.addHover(5.0, "wait_for_command");
    
    // 7. 手动控制阶段（用于精细操作）
    manager.addTopicControl("/operator_control", 30.0, "manual_operation");
    
    // 8. 返回基地
    manager.addMovePoint(0.0, 0.0, 2.0, "return_to_base");
    
    // 9. 最终朝向调整
    manager.addRotation(M_PI/2, "final_orientation");
    
    // 10. 安全降落
    manager.addSafeLanding("safe_landing");
    
    manager.finalize();
}

// 示例：巡逻任务
void createPatrolMission(WaypointManager& manager) {
    ROS_WARN("Creating Patrol Mission...");
    manager.reset();
    
    // 第一个任务：起飞到1.5米高度
    manager.addMovePoint(0.0, 0.0, 1.5, "takeoff_to_height");
    
    // 四角巡逻
    manager.addMovePoint(3.0, 0.0, 1.5, "patrol_point_1");
    manager.addRotation(M_PI/2, "scan_right");
    manager.addHover(3.0, "observe_1");
    
    manager.addMovePoint(3.0, 3.0, 1.5, "patrol_point_2");
    manager.addRotation(M_PI, "scan_back");
    manager.addHover(3.0, "observe_2");

    manager.addMovePoint(0.0, 3.0, 1.5, "patrol_point_3");
    manager.addRotation(-M_PI/2, "scan_left");
    manager.addHover(3.0, "observe_3");
    
    manager.addMovePoint(0.0, 0.0, 1.5, "patrol_point_4");
    manager.addRotation(0.0, "scan_front");
    manager.addHover(3.0, "observe_4");

    manager.addMovePoint(0.0, 0.0, 1.5, "patrol_point_5");

    
    // 添加安全降落作为最后一个动作
    manager.addSafeLanding("safe_landing");
    
    manager.finalize();  // 完成初始化
    ROS_WARN("Patrol Mission Created successfully!");
}

// 示例：训练模式（结合手动和自动）
void createTrainingMission(WaypointManager& manager) {
    manager.reset();
    
    // 起飞到训练高度
    manager.addMovePoint(0.0, 0.0, 1.5, "takeoff_to_training_height");
    
    // 自动飞到训练区域
    manager.addMovePoint(5.0, 5.0, 1.5, "training_area");
    
    // 手动训练阶段1：基础移动
    manager.addTopicControl("/trainee_control", 60.0, "basic_movement_training");
    
    // 自动演示：圆形飞行
    manager.addCircleFlight(5.0, 5.0, 1.5, 2.0, 10.0, "demo_circle_flight");
    
    // 手动训练阶段2：高级机动
    manager.addTopicControl("/trainee_control", 120.0, "advanced_maneuver_training");
    
    // 返回起点
    manager.addMovePoint(0.0, 0.0, 1.5, "training_complete");
    
    // 安全降落
    manager.addSafeLanding("safe_landing");
}

// 示例：起飞任务
void createTakeoffMission(WaypointManager& manager, double targetHeight = 1.5) {
    ROS_WARN("Creating Takeoff Mission to %.1f meters...", targetHeight);
    manager.reset();
    
    // 起飞到指定高度
    manager.addMovePoint(0.0, 0.0, targetHeight, "takeoff_to_height");
    
    manager.finalize();  // 完成初始化
    ROS_WARN("Takeoff Mission Created successfully!");
}

int main(int argc, char** argv)
{
    double target_speed=0.5;
    int rotation_direction=0;
    std_msgs::ColorRGBA lidar_data_sender;
    double* pole_data;
    double yaw_angle,start_yaw;
    std_msgs::Int8 vision_cmd_sender;
    std_msgs::Int16 led_data_sender;
    led_data_sender.data = 0;
    vision_cmd_sender.data = 1;
    
    // 创建航路点管理器并设置初始任务
    WaypointManager waypointManager;
    // 可以使用预定义的任务
    // createComplexMission(waypointManager);  // 搜索救援任务
    createPatrolMission(waypointManager);      // 启用巡逻任务 - 取消注释
    // createTrainingMission(waypointManager); // 训练任务
    
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;
    // 订阅命令消息

    // 订阅飞行器状态和发布位置设定点
    state_sub = nh.subscribe<mavros_msgs::State>
        ("/mavros/state", 10, state_cb);  //订阅状态
    pose_sub = nh.subscribe<geometry_msgs::PoseStamped>
        ("/mavros/local_position/pose", 10, pose_cb); //订阅位置信息.
    vel_sub = nh.subscribe<geometry_msgs::TwistStamped> 
        ("/mavros/local_position/velocity", 10 ,vel_cb);
    local_pose_pub = nh.advertise<geometry_msgs::PoseStamped>
        ("/mavros/setpoint_position/local", 10);  //设置位置
    local_vel_pub = nh.advertise<geometry_msgs::TwistStamped>
        ("/mavros/setpoint_velocity/cmd_vel", 10);  //设定速度
    set_raw_pub = nh.advertise<mavros_msgs::PositionTarget>
        ("/mavros/setpoint_raw/local", 10); //以自身为坐标系的速度控制
    state_message_pub = nh.advertise<std_msgs::Int8>
        ("/offboard_node/state",10);
    // 定义服务客户端，用于解锁/上锁无人机和切换到离线控制模式
    arming_client = nh.serviceClient<mavros_msgs::CommandBool>
        ("/mavros/cmd/arming");   
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
        ("/mavros/set_mode");
    ros::ServiceClient land_client = nh.serviceClient<mavros_msgs::CommandTOL>
        ("/mavros/cmd/land");
    ros::ServiceServer state_client = nh.advertiseService
        ("/command",state_cb);    

    // 指定发布位置设定点的频率
    ros::Rate rate(50.0);  

    // 等待 FCU 连接
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    // 发布一些起始位置设定点，然后才开始执行控制指令
    geometry_msgs::TwistStamped vector;
    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_vel_pub.publish(vector);
        ros::spinOnce();
        rate.sleep();
    }

    // 请求进入离线控制模式
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    // 请求解锁飞行器
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    // 记录上一次请求时间
    ros::Time last_request = ros::Time::now();
    ros::Time last_time = ros::Time::now();
    ros::Time fire_time = ros::Time::now();
    mavros_msgs::CommandTOL land_cmd;
    std_msgs::Int8 offboard_state;
    std_msgs::Int8 drop_command;
    drop_command.data = 0;
    geometry_msgs::PoseStamped target_position;
    geometry_msgs::PoseStamped landing_position;
    geometry_msgs::PoseStamped rounding_pose[3];
    land_cmd.request.altitude = 0.0;
    land_cmd.request.yaw = 0.0;

    // 用于步骤
    int flag = 0;
  
    // 定义class的实例
    move_map move1;
    
    while(ros::ok())
    {
        ros::spinOnce(); 
		// ROS_INFO("Pose-z: %f",current_pose.pose.position.z);
		// 无人机状态设定与判断      
        // 进入while循环后，先循环5s，然后再向客户端发送无人机状态设置的消息
        // set_mode_client.call   arming_client.call 
        if( current_state.mode != "OFFBOARD" &&(ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&offb_set_mode.response.mode_sent){
                ROS_WARN("Offboard enabled");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_WARN("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
            else    //  无人机 Offboard enabled && Vehicle armed 后
            {        
                // 使用增强版链表优化的航路点控制 - 统一任务管理
                static bool mission_initialized = false;
                
                // 一次性初始化任务系统
                if (!mission_initialized) {
                    flag = 0;
                    last_time = ros::Time::now();
                    move1.pid_init();
                    move1.set_pid_xy(0.8,0.1,0.2);
                    move1.set_pid_z(0.8,0.0,0.0);
                    move1.set_height(TargeyHeight);
                    mission_initialized = true;
                    
                    // 开始第一个航路点任务
                    if (waypointManager.hasNextWaypoint()) {
                        Waypoint currentWP = waypointManager.getCurrentWaypoint();
                        ROS_WARN("Mission started! First task: %s (Action: %d)", currentWP.stepName.c_str(), currentWP.actionType);
                    }
                }
                
                // 执行航路点导航
                if (waypointManager.hasNextWaypoint()) {
                    Waypoint currentWP = waypointManager.getCurrentWaypoint();
                    
                    // 为话题控制设置订阅
                    if (currentWP.actionType == TOPIC_CONTROL) {
                        waypointManager.setupTopicSubscription(nh, currentWP.topicName);
                    }

                    bool actionComplete = waypointManager.isCurrentActionComplete(move1, local_vel_pub, rate);

                    if (actionComplete) {
                        last_time = ros::Time::now();
                        ROS_INFO("Completed %s", currentWP.stepName.c_str());
                        
                        // 移动到下一个航路点
                        waypointManager.moveToNextWaypoint();
                        
                        if (waypointManager.hasNextWaypoint()) {
                            Waypoint nextWP = waypointManager.getCurrentWaypoint();
                            ROS_INFO("Starting %s (Action: %d)", nextWP.stepName.c_str(), nextWP.actionType);
                            ROS_WARN("Starting %s (Action: %d)", nextWP.stepName.c_str(), nextWP.actionType);
                        } else {
                            // 所有航路点完成，任务结束
                            ROS_INFO("All waypoints completed, mission finished");
                            ROS_WARN("All waypoints completed, mission finished");
                        }
                    }
                } else {
                    ROS_WARN("No more waypoints, mission finished");
                }
            }
        }
        rate.sleep();
    }
    return 0;
}
